#include <mutex>
#include <memory>
#include <iostream>
#include <cmath>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <eigen_conversions/eigen_msg.h>
#include <eigen3/Eigen/Dense>

#include <std_srvs/Empty.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/filters/voxel_grid.h>

#include <pclomp/ndt_omp.h>
#include <fast_gicp/ndt/ndt_cuda.hpp>

#include <hdl_localization/pose_estimator.hpp>
#include <hdl_localization/delta_estimater.hpp>

#include <hdl_localization/ScanMatchingStatus.h>
#include <hdl_global_localization/SetGlobalMap.h>
#include <hdl_global_localization/QueryGlobalLocalization.h>


#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <hdl_localization/registrations.hpp>

#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <geographic_msgs/GeoPointStamped.h>

struct pose_type {
	double x;  // Ä¬ÈÏµ¥Î»Îªm£¨»òm/s£©
	double y;  // Ä¬ÈÏµ¥Î»Îªm£¨»òm/s£©
	double theta; // Ä¬ÈÏµ¥Î»Îªrad£¨»òrad/s£©
};

namespace hdl_localization {

class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, pcl::PointCloud<PointT>> ApproxSyncPolicy;

  HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) {
  }
  virtual ~HdlLocalizationNodelet() {
  }

  void onInit() override {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    initialize_params();

    robot_odom_frame_id = private_nh.param<std::string>("robot_odom_frame_id", "robot_odom");
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");

    use_imu = private_nh.param<bool>("use_imu", true);
    invert_acc = private_nh.param<bool>("invert_acc", false);
    invert_gyro = private_nh.param<bool>("invert_gyro", false);

    use_gps = private_nh.param<bool>("use_gps", true);
    
    if (use_imu) {
      NODELET_INFO("enable imu-based prediction");
      imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this);
    }

    if (use_gps)
      {
        NODELET_INFO("enable gps location");        
        heading_sub = nh.subscribe("/fdi_imu", 0.2, &HdlLocalizationNodelet::heading_callback, this);
        gnss_sub = nh.subscribe("/gps/fix", 0.2, &HdlLocalizationNodelet::navsat_callback, this);
        gps_sub_sync.reset(new message_filters::Subscriber<sensor_msgs::NavSatFix>(mt_nh, "/gps/fix", 256));
        cloud_sub_sync.reset(new message_filters::Subscriber<pcl::PointCloud<PointT>>(mt_nh, "/velodyne_points", 32));
	  sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *gps_sub_sync, *cloud_sub_sync));
	  sync->registerCallback(boost::bind(&HdlLocalizationNodelet::points_callback_sync, this, _1, _2));	
      }
    else
      {
        points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
      }
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);

    pose_pub = nh.advertise<nav_msgs::Odometry>("/odom", 5, false);
    aligned_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 5, false);
    status_pub = nh.advertise<ScanMatchingStatus>("/status", 5, false);
    gps_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("gps_pose", 5, false);
    result_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("laserpose", 5, false);
    
    // global localization
    use_global_localization = private_nh.param<bool>("use_global_localization", true);
    if(use_global_localization) {
      NODELET_INFO_STREAM("wait for global localization services");
      ros::service::waitForService("/hdl_global_localization/set_global_map");
      ros::service::waitForService("/hdl_global_localization/query");

      set_global_map_service = nh.serviceClient<hdl_global_localization::SetGlobalMap>("/hdl_global_localization/set_global_map");
      query_global_localization_service = nh.serviceClient<hdl_global_localization::QueryGlobalLocalization>("/hdl_global_localization/query");

      relocalize_server = nh.advertiseService("/relocalize", &HdlLocalizationNodelet::relocalize, this);
    }
  }

private:
  pcl::Registration<PointT, PointT>::Ptr create_registration() const {
    std::string reg_method = private_nh.param<std::string>("reg_method", "NDT_OMP");
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");
    double ndt_neighbor_search_radius = private_nh.param<double>("ndt_neighbor_search_radius", 2.0);
    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);

    if(reg_method == "NDT_OMP") {
      NODELET_INFO("NDT_OMP is selected");
      pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
      ndt->setTransformationEpsilon(0.01);
      ndt->setResolution(ndt_resolution);      
      if (ndt_neighbor_search_method == "DIRECT1") {
        NODELET_INFO("search_method DIRECT1 is selected");
        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      } else if (ndt_neighbor_search_method == "DIRECT7") {
        NODELET_INFO("search_method DIRECT7 is selected");
        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      } else {
        if (ndt_neighbor_search_method == "KDTREE") {
          NODELET_INFO("search_method KDTREE is selected");
        } else {
          NODELET_WARN("invalid search method was given");
          NODELET_WARN("default method is selected (KDTREE)");
        }
        ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      }
      return ndt;
    } else if(reg_method.find("NDT_CUDA") != std::string::npos) {
      NODELET_INFO("NDT_CUDA is selected");
      boost::shared_ptr<fast_gicp::NDTCuda<PointT, PointT>> ndt(new fast_gicp::NDTCuda<PointT, PointT>);
      ndt->setResolution(ndt_resolution);

      if(reg_method.find("D2D") != std::string::npos) {
        ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D);
      } else if (reg_method.find("P2D") != std::string::npos) {
        ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D);
      }

      if (ndt_neighbor_search_method == "DIRECT1") {
        NODELET_INFO("search_method DIRECT1 is selected");
        ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT1);
      } else if (ndt_neighbor_search_method == "DIRECT7") {
        NODELET_INFO("search_method DIRECT7 is selected");
        ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT7);
      } else if (ndt_neighbor_search_method == "DIRECT_RADIUS") {
        NODELET_INFO_STREAM("search_method DIRECT_RADIUS is selected : " << ndt_neighbor_search_radius);
        ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT_RADIUS, ndt_neighbor_search_radius);
      } else {
        NODELET_WARN("invalid search method was given");
      }
      return ndt;
    }

    NODELET_ERROR_STREAM("unknown registration method:" << reg_method);
    return nullptr;
  }

  void initialize_params() {
    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;


    // ***************lidar odometry*********************
    //downsample
    auto voxelgrid_lio = new pcl::VoxelGrid<PointT>();
    voxelgrid_lio->setLeafSize(0.5, 0.5, 0.5);
    downsample_filter_lio.reset(voxelgrid_lio);
    //outlier_removal
    std::string outlier_removal_method = private_nh.param<std::string>("outlier_removal_method", "STATISTICAL");
    if(outlier_removal_method == "STATISTICAL") {
      int mean_k = private_nh.param<int>("statistical_mean_k", 20);
      double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);
      std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;

      pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());
      sor->setMeanK(mean_k);
      sor->setStddevMulThresh(stddev_mul_thresh);
      outlier_removal_filter_lio = sor;
    } else if(outlier_removal_method == "RADIUS") {
      double radius = private_nh.param<double>("radius_radius", 0.8);
      int min_neighbors = private_nh.param<int>("radius_min_neighbors", 2);
      std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;

      pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
      rad->setRadiusSearch(radius);
      rad->setMinNeighborsInRadius(min_neighbors);
      outlier_removal_filter_lio = rad;
    } else {
      std::cout << "outlier_removal: NONE" << std::endl;
    }
    //registration
    registration_lio = select_registration_method(private_nh);
    //distance filter
    use_distance_filter = private_nh.param<bool>("use_distance_filter", true);
    distance_near_thresh = private_nh.param<double>("distance_near_thresh", 1.0);
    distance_far_thresh = private_nh.param<double>("distance_far_thresh", 100.0);

    
    NODELET_INFO("create registration method for localization");
    registration = create_registration();

    // global localization
    NODELET_INFO("create registration method for fallback during relocalization");
    relocalizing = false;
    delta_estimater.reset(new DeltaEstimater(create_registration()));

    // initialize pose estimator
    if(private_nh.param<bool>("specify_init_pose", true)) {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaternionf(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
        private_nh.param<double>("cool_time_duration", 0.5)
      ));
    }

    // gps initialization
    std::string utm_file_add = private_nh.param<std::string>("utm_file", "");
    std::ifstream utm_file(utm_file_add);
    if (utm_file.is_open() && private_nh.param<bool>("use_utm", true) && (private_nh.param<bool>("use_gps", true))) {
      
      utm_file >> zero_utm[0] >> zero_utm[1] >> zero_utm[2] >> zero_yaw;
      
      Eigen::AngleAxisd v(M_PI * (zero_yaw - 90.0) / 180.0, Eigen::Vector3d::UnitZ());
      rotMatrix = v.matrix();

      gnss_init = true;
      ROS_INFO("gnss_init ok");
    }
    gps_2_lidar.x = private_nh.param<double>("gps_2_lidar_x", 0.0);
    gps_2_lidar.y = private_nh.param<double>("gps_2_lidar_y", 0.0);
    gps_2_lidar.theta = private_nh.param<double>("gps_2_lidar_th", 0.0);
  }

private:
  /**
   * @brief callback for imu data
   * @param imu_msg
   */
  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {
    std::lock_guard<std::mutex> lock(imu_data_mutex);
    imu_data.push_back(imu_msg);
  }

  pose_type pose_transform(const pose_type* inital_pose, const pose_type* transform_pose) {
    double delta_theta = transform_pose->theta;
    double cos_delta_theta = cos(delta_theta);
    double sin_delta_theta = sin(delta_theta);

    pose_type result_pose;

    result_pose.x = transform_pose->x + inital_pose->x*cos_delta_theta - inital_pose->y*sin_delta_theta;
    result_pose.y = transform_pose->y + inital_pose->x*sin_delta_theta + inital_pose->y*cos_delta_theta;
    result_pose.theta = delta_theta + inital_pose->theta;

    result_pose.theta = atan2(sin(result_pose.theta),cos(result_pose.theta));

    return result_pose;
  }
  
  // global_poseÎªÈ«¾ÖÎ»×Ë£¬deltaÎª¾Ö²¿Î»×Ë,·µ»ØÆ«ÒÆ²ÎÊý
pose_type pose_inv_transform(const pose_type* global_pose, const pose_type* delta)
{
  pose_type result_pose;
  result_pose.theta = global_pose->theta - delta->theta;
  result_pose.theta = atan2(sin(result_pose.theta),cos(result_pose.theta));
  double cos_delta_theta = cos(result_pose.theta);
  double sin_delta_theta = sin(result_pose.theta);

  result_pose.x = global_pose->x - delta->x*cos_delta_theta + delta->y*sin_delta_theta;
  result_pose.y = global_pose->y - delta->x*sin_delta_theta - delta->y*cos_delta_theta;

  return result_pose;
}


// global_poseÎªÈ«¾ÖÎ»×Ë£¬biasÎªÆ«ÒÆÎ»×Ë,·µ»Ø¾Ö²¿Î»×Ë
pose_type pose_delta_transform(const pose_type* global_pose, const pose_type* bias)
{
  pose_type result_pose;
  result_pose.theta = global_pose->theta - bias->theta;
  result_pose.theta = atan2(sin(result_pose.theta),cos(result_pose.theta));
  double cos_delta_theta = cos(bias->theta);
  double sin_delta_theta = sin(bias->theta);

  result_pose.x = (global_pose->x - bias->x)*cos_delta_theta + (global_pose->y - bias->y)*sin_delta_theta;
  result_pose.y = -(global_pose->x - bias->x)*sin_delta_theta + (global_pose->y - bias->y)*cos_delta_theta;

  return result_pose;
}

  /**
   * @brief async callback for point cloud data and gps data
   * @param points_msg
   */
  void points_callback_sync(const sensor_msgs::NavSatFixConstPtr& navsat_msg, const pcl::PointCloud<PointT>::ConstPtr& points_msg)
  {
    std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
    if(!pose_estimator) {
      NODELET_ERROR("waiting for initial pose input!!");
      return;
    }

    if(!globalmap) {
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }
    if (!gnss_init)
    {
      NODELET_INFO("gnss initialization is not OK!!!");
      return;
    }
    
    //pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
    //pcl::fromROSMsg(*points_msg, *pcl_cloud);
    pcl::PointCloud<PointT>::ConstPtr pcl_cloud = points_msg->makeShared();
    
    if(points_msg->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }
    if(!navsat_msg){
      NODELET_ERROR("gps msg is empty!!");
      return;
    }

    ros::Time stamp = pcl_conversions::fromPCL(pcl_cloud->header.stamp);
    
    auto t1 = ros::WallTime::now();
    //pcl_cloud = lio_deskewing(pcl_cloud);
    auto t2 = ros::WallTime::now();
    //std::cout<<"lio cost time is:"<<(t2-t1).toSec()<<"sec."<<std::endl;
      
    

    // transform pointcloud into odom_child_frame_id
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) {
      NODELET_ERROR("point cloud cannot be transformed into target frame!!");
      return;
    }

    auto filtered = downsample(cloud);
    last_scan = filtered;

    if(relocalizing) {
      delta_estimater->add_frame(filtered);
    }

    Eigen::Matrix4f before = pose_estimator->matrix();
    
    // predict
    if(!use_imu) {
      pose_estimator->predict(stamp);
      
    } else {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      auto imu_iter = imu_data.begin();
      for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
        if(stamp < (*imu_iter)->header.stamp) {
          break;
        }
        const auto& acc = (*imu_iter)->linear_acceleration;
        const auto& gyro = (*imu_iter)->angular_velocity;
        double acc_sign = invert_acc ? -1.0 : 1.0;
        double gyro_sign = invert_gyro ? -1.0 : 1.0;
        pose_estimator->predict((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
      }
      imu_data.erase(imu_data.begin(), imu_iter);
    }

    // odometry-based prediction
    ros::Time last_correction_time = pose_estimator->last_correction_time();
    if(private_nh.param<bool>("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) {
      geometry_msgs::TransformStamped odom_delta;
      if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) {
        odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0));
      } else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) {
        odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0));
      }

      if(odom_delta.header.stamp.isZero()) {
        NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id);
      } else {
        Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta);
        pose_estimator->predict_odom(delta.cast<float>().matrix());
      }
    }

    t1 = ros::WallTime::now();

    // correct
    auto aligned = pose_estimator->correct(stamp, filtered);

    t2 = ros::WallTime::now();

    //std::cout<<"pose correct cost time is:"<<(t2-t1).toSec()<<" sec"<<std::endl;
    
    if(aligned_pub.getNumSubscribers()) {
      aligned->header.frame_id = "map";
      aligned->header.stamp = cloud->header.stamp;
      aligned_pub.publish(aligned);
    }

    if(status_pub.getNumSubscribers()) {
      publish_scan_matching_status(pcl_conversions::fromPCL(pcl_cloud->header), aligned);
    }

    //Analysis matchmap pose    
    Eigen::Matrix4f match_pose_matrix(pose_estimator->matrix());
    tf::Matrix3x3 mat_l;
    mat_l.setValue(static_cast<double>(match_pose_matrix(0, 0)),static_cast<double>(match_pose_matrix(0, 1)),static_cast<double>(match_pose_matrix(0, 2)),
		   static_cast<double>(match_pose_matrix(1, 0)),static_cast<double>(match_pose_matrix(1, 1)),static_cast<double>(match_pose_matrix(1, 2)),
		   static_cast<double>(match_pose_matrix(2, 0)),static_cast<double>(match_pose_matrix(2, 1)),static_cast<double>(match_pose_matrix(2, 2))
    );
    geometry_msgs::PoseWithCovarianceStamped matchmap_pose;
    matchmap_pose.header.frame_id = "map";
    matchmap_pose.header.stamp = navsat_msg->header.stamp;

    matchmap_pose.pose.pose.position.x = static_cast<double>(match_pose_matrix(0,3));   //std::cout<<"x:"<< match_pose_matrix(0,3)<<std::endl;
    matchmap_pose.pose.pose.position.y = static_cast<double>(match_pose_matrix(1,3));   //std::cout<<"y:"<< match_pose_matrix(1,3)<<std::endl;
    matchmap_pose.pose.pose.position.z = static_cast<double>(match_pose_matrix(2,3));   //std::cout<<"z:"<< match_pose_matrix(2,3)<<std::endl;

    double m_p_roll, m_p_pitch, m_p_yaw;
    mat_l.getRPY(m_p_roll, m_p_pitch, m_p_yaw);
    tf2::Quaternion m_q;
    m_q.setRPY(m_p_roll, m_p_pitch, m_p_yaw);
    tf2::convert(m_q, matchmap_pose.pose.pose.orientation);
    matchmap_pose.pose.covariance[0] = 0.05 * 0.05;
    matchmap_pose.pose.covariance[1] = 0.0;
    matchmap_pose.pose.covariance[6] = 0.0;
    matchmap_pose.pose.covariance[7] = 0.05 * 0.05;     
    matchmap_pose.pose.covariance[6*5 + 5] = 0.01 * 0.01;

    //Analysis GPS pose
    std::lock_guard<std::mutex> lock(q_heading_mutex);
    
    Eigen::Quaternionf q_heading_ = q_heading;

    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = navsat_msg->header;
    gps_msg->position.latitude = navsat_msg->latitude;
    gps_msg->position.longitude = navsat_msg->longitude;
    gps_msg->position.altitude = navsat_msg->altitude;
    
    geodesy::fromMsg(gps_msg->position, utm);
    Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

    xyz -= zero_utm;
    
    double theta = zero_yaw / 180.0 * M_PI;
    double temp_xyz0 =  cos(theta) * xyz[0] + sin(theta) * xyz[1];
    double temp_xyz1 = -sin(theta) * xyz[0] + cos(theta) * xyz[1];
    xyz[0] = temp_xyz0;
    xyz[1] = temp_xyz1;

    geometry_msgs::Quaternion gps_temp_quaternion;
    gps_temp_quaternion.x = q_heading_.x();
    gps_temp_quaternion.y = q_heading_.y();
    gps_temp_quaternion.z = q_heading_.z();
    gps_temp_quaternion.w = q_heading_.w();
    pose_type temp_pose;//+
    temp_pose.x = xyz[0];temp_pose.y = xyz[1];temp_pose.theta = tf::getYaw(gps_temp_quaternion);//+
    temp_pose = pose_inv_transform(&temp_pose, &gps_2_lidar);//+
    temp_pose = pose_transform(&temp_pose, &gps_2_lidar);//+
    xyz[0] = temp_pose.x;//+
    xyz[1] = temp_pose.y;//+
    geometry_msgs::Quaternion temp_quaternion = tf::createQuaternionMsgFromYaw(temp_pose.theta);//+
    q_heading_.x() = temp_quaternion.x;//+
    q_heading_.y() = temp_quaternion.y;//+
    q_heading_.z() = temp_quaternion.z;//+
    q_heading_.w() = temp_quaternion.w;//+

    //geometry_msgs::Quaternion gps_temp_quaternion;
    gps_temp_quaternion.x = q_heading_.x();
    gps_temp_quaternion.y = q_heading_.y();
    gps_temp_quaternion.z = q_heading_.z();
    gps_temp_quaternion.w = q_heading_.w();

    cur_gps_pose[0] = xyz[0];cur_gps_pose[1] = xyz[1];cur_gps_pose[2] = tf::getYaw(gps_temp_quaternion);
    
    double cur_gps_pose_x, cur_gps_pose_y, cur_gps_pose_theta;
    if(first_in_match)
    {
       cur_gps_pose_x = cur_gps_pose[0];
       cur_gps_pose_y = cur_gps_pose[1];
       cur_gps_pose_theta = cur_gps_pose[2];
              
       first_in_match = false;
    }
    else
    { 
       double d_gps_x = cos(prev_gps_pose[2])*(cur_gps_pose[0] - prev_gps_pose[0]) + sin(prev_gps_pose[2])*(cur_gps_pose[1] - prev_gps_pose[1]);
       double d_gps_y = -sin(prev_gps_pose[2])*(cur_gps_pose[0] - prev_gps_pose[0]) + cos(prev_gps_pose[2])*(cur_gps_pose[1] - prev_gps_pose[1]);
       double d_gps_th = tf::getYaw(gps_temp_quaternion) - prev_gps_pose[2];
       d_gps_th = atan2(sin(d_gps_th), cos(d_gps_th));
    
       cur_gps_pose_x = cos(prev_pose[2])*d_gps_x - sin(prev_pose[2])*d_gps_y + prev_pose[0];
       cur_gps_pose_y = sin(prev_pose[2])*d_gps_x + cos(prev_pose[2])*d_gps_y + prev_pose[1];
       cur_gps_pose_theta = prev_pose[2] + d_gps_th;
       cur_gps_pose_theta = atan2(sin(cur_gps_pose_theta), cos(cur_gps_pose_theta));
    }
    //publish RTK pose,TODO:GPS data valid or invalid jurdge
    geometry_msgs::PoseWithCovarianceStamped gps_pose;
    // Fill in the header
    gps_pose.header.frame_id = "map";
    gps_pose.header.stamp = navsat_msg->header.stamp;
    // Copy in the pose
    gps_pose.pose.pose.position.x = cur_gps_pose_x;
    gps_pose.pose.pose.position.y = cur_gps_pose_y;

 
    tf2::Quaternion q;
    q.setRPY(0, 0, cur_gps_pose_theta);
    tf2::convert(q, gps_pose.pose.pose.orientation);
    
    gps_pose.pose.covariance[0] = 0.02 * 0.02;
    gps_pose.pose.covariance[1] = 0.0;
    gps_pose.pose.covariance[6] = 0.0;
    gps_pose.pose.covariance[7] = 0.02 * 0.02;     
    gps_pose.pose.covariance[6*5 + 5] = 0.01 * 0.01;

        
    //Calculate inlier index
    const double max_correspondence_dist = 0.5;

    int num_inliers = 0;
    std::vector<int> k_indices;
    std::vector<float> k_sq_dists;
    
    for(int i = 0; i < aligned->size(); i++) {
      const auto& pt = aligned->at(i);
      registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
      if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
        num_inliers++;
      }
    }
    float inlier_fraction = static_cast<float>(num_inliers) / aligned->size();

    //Get match mean error index
    float match_mean_error = registration->getFitnessScore( 50.0 );
    

    std::cout<<"Match error is:"<<match_mean_error<<"[m]. Registration Converged flag is:"<<registration->hasConverged()<<",and inlier ratio is:"<<inlier_fraction<<std::endl;
    
    std::cout<<"Match pose is:"<<matchmap_pose.pose.pose.position.x<<","<<matchmap_pose.pose.pose.position.y<<","<<m_p_yaw*57.3<<std::endl;
    
    std::cout<<"GPS pose is:"<<cur_gps_pose_x<<","<<cur_gps_pose_y<<","<<cur_gps_pose_theta*57.3<<std::endl;
    
    //Jurge which pose to published: Gps-pose or scan-to-map-pose, need to Think!!!
    if(match_mean_error > 2.0 && (int)navsat_msg->status.status == 1)//ScanMatch not valid,GPS is valid
      {
	std::cout<<"publish gps pose:"<<xyz[0]<<","<<xyz[1]<<","<<tf::getYaw(gps_temp_quaternion)<<std::endl;
	
	result_pose_pub.publish(gps_pose);

	prev_pose[0] = cur_gps_pose[0];
	prev_pose[1] = cur_gps_pose[1];
	prev_pose[2] = cur_gps_pose[2];
	
	pose_estimator.reset(new hdl_localization::PoseEstimator(registration, ros::Time::now(), Eigen::Vector3f(xyz[0], xyz[1], 0), q_heading_, private_nh.param<double>("cool_time_duration", 0.5)));

      }
    else if(match_mean_error <= 2.0)//ScanMatch is valid
      {
	result_pose_pub.publish(matchmap_pose);

	prev_pose[0] = matchmap_pose.pose.pose.position.x;
	prev_pose[1] = matchmap_pose.pose.pose.position.y;
	prev_pose[2] = m_p_yaw;       		
      }
    prev_gps_pose[0] = cur_gps_pose[0];
    prev_gps_pose[1] = cur_gps_pose[1];
    prev_gps_pose[2] = cur_gps_pose[2];
  }



  
  /**
   * @brief callback for point cloud data
   * @param points_msg
   */
  void points_callback(const pcl::PointCloud<PointT>& points_msg) {
    std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
    if(!pose_estimator) {
      NODELET_ERROR("waiting for initial pose input!!");
      return;
    }

    if(!globalmap) {
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }
    
    //pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
    //pcl::fromROSMsg(*points_msg, *pcl_cloud);
    pcl::PointCloud<PointT>::ConstPtr pcl_cloud = points_msg.makeShared();
    
    if(pcl_cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }

    ros::Time stamp = pcl_conversions::fromPCL(pcl_cloud->header.stamp);
    
    auto t1 = ros::WallTime::now();
    //pcl_cloud = lio_deskewing(pcl_cloud);
    auto t2 = ros::WallTime::now();
    //std::cout<<"lio cost time is:"<<(t2-t1).toSec()<<"sec."<<std::endl;
      
    

    // transform pointcloud into odom_child_frame_id
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) {
        NODELET_ERROR("point cloud cannot be transformed into target frame!!");
        return;
    }

    auto filtered = downsample(cloud);
    last_scan = filtered;

    if(relocalizing) {
      delta_estimater->add_frame(filtered);
    }

    Eigen::Matrix4f before = pose_estimator->matrix();
    
    // predict
    if(!use_imu) {
      pose_estimator->predict(stamp);      
    } else {
      std::lock_guard<std::mutex> lock(imu_data_mutex);
      auto imu_iter = imu_data.begin();
      for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
        if(stamp < (*imu_iter)->header.stamp) {
          break;
        }
        const auto& acc = (*imu_iter)->linear_acceleration;
        const auto& gyro = (*imu_iter)->angular_velocity;
        double acc_sign = invert_acc ? -1.0 : 1.0;
        double gyro_sign = invert_gyro ? -1.0 : 1.0;
        pose_estimator->predict((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
      }
      imu_data.erase(imu_data.begin(), imu_iter);
    }

    // odometry-based prediction
    ros::Time last_correction_time = pose_estimator->last_correction_time();
    if(private_nh.param<bool>("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) {
      geometry_msgs::TransformStamped odom_delta;
      if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) {
        odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0));
      } else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) {
        odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0));
      }

      if(odom_delta.header.stamp.isZero()) {
        NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id);
      } else {
        Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta);
        pose_estimator->predict_odom(delta.cast<float>().matrix());
      }
    }

    t1 = ros::WallTime::now();

    // correct
    auto aligned = pose_estimator->correct(stamp, filtered);

    t2 = ros::WallTime::now();

    //std::cout<<"pose correct cost time is:"<<(t2-t1).toSec()<<" sec"<<std::endl;
    
    if(aligned_pub.getNumSubscribers()) {
      aligned->header.frame_id = "map";
      aligned->header.stamp = cloud->header.stamp;
      aligned_pub.publish(aligned);
    }

    if(status_pub.getNumSubscribers()) {
      publish_scan_matching_status(pcl_conversions::fromPCL(pcl_cloud->header), aligned);
    }

    const double max_correspondence_dist = 0.5;

    int num_inliers = 0;
    std::vector<int> k_indices;
    std::vector<float> k_sq_dists;
    for(int i = 0; i < aligned->size(); i++) {
      const auto& pt = aligned->at(i);
      registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
      if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
        num_inliers++;
      }
    }
    float inlier_fraction = static_cast<float>(num_inliers) / aligned->size();
    
    std::cout<<"Match error is:"<<registration->getFitnessScore(50)<<"[m]. registration Converged flag is:"<<registration->hasConverged()<<",and inlier ratio is:"<<inlier_fraction<<std::endl;
    
    //publish_odometry(stamp, pose_estimator->matrix());

    //Analysis matchmap pose    
    Eigen::Matrix4f match_pose_matrix(pose_estimator->matrix());
    tf::Matrix3x3 mat_l;
    mat_l.setValue(static_cast<double>(match_pose_matrix(0, 0)),static_cast<double>(match_pose_matrix(0, 1)),static_cast<double>(match_pose_matrix(0, 2)),
		   static_cast<double>(match_pose_matrix(1, 0)),static_cast<double>(match_pose_matrix(1, 1)),static_cast<double>(match_pose_matrix(1, 2)),
		   static_cast<double>(match_pose_matrix(2, 0)),static_cast<double>(match_pose_matrix(2, 1)),static_cast<double>(match_pose_matrix(2, 2))
);
    geometry_msgs::PoseWithCovarianceStamped matchmap_pose;
    matchmap_pose.header.frame_id = "map";
    matchmap_pose.header.stamp = stamp;

    matchmap_pose.pose.pose.position.x = static_cast<double>(match_pose_matrix(0,3));   //std::cout<<"x:"<< match_pose_matrix(0,3)<<std::endl;
    matchmap_pose.pose.pose.position.y = static_cast<double>(match_pose_matrix(1,3));   //std::cout<<"y:"<< match_pose_matrix(1,3)<<std::endl;
    matchmap_pose.pose.pose.position.z = static_cast<double>(match_pose_matrix(2,3));   //std::cout<<"z:"<< match_pose_matrix(2,3)<<std::endl;

    double m_p_roll, m_p_pitch, m_p_yaw;
    mat_l.getRPY(m_p_roll, m_p_pitch, m_p_yaw);
    tf2::Quaternion m_q;
    m_q.setRPY(m_p_roll, m_p_pitch, m_p_yaw);
    tf2::convert(m_q, matchmap_pose.pose.pose.orientation);
    matchmap_pose.pose.covariance[0] = 0.05 * 0.05;
    matchmap_pose.pose.covariance[1] = 0.0;
    matchmap_pose.pose.covariance[6] = 0.0;
    matchmap_pose.pose.covariance[7] = 0.05 * 0.05;     
    matchmap_pose.pose.covariance[6*5 + 5] = 0.01 * 0.01;
    result_pose_pub.publish(matchmap_pose);
  }


  /**
   * @brief lidar odometry for deskewing
   * @param cloud
   */
  pcl::PointCloud<PointT>::ConstPtr lio_deskewing(const pcl::PointCloud<PointT>::ConstPtr& cloud) {

    auto t1 = ros::WallTime::now();
    pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(cloud);
    filtered = downsample_lio(filtered);    
    filtered = outlier_removal(filtered);
    auto t2 = ros::WallTime::now();

    //std::cout<<"precess cost time is:"<<(t2-t1).toSec()<<std::endl;
    
    if(first_in_lio) {
      first_in_lio = false;
      prev_odom.setIdentity();
      odom.setIdentity();
      
      registration_lio->setInputTarget(filtered);

      return cloud;
    }

    t1 = ros::WallTime::now();
    registration_lio->setInputSource(filtered);

    
    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    registration_lio->align(*aligned, prev_odom);
    
    
    odom = registration_lio->getFinalTransformation();


    double dx = odom.block<3, 1>(0, 3).norm();
    double da = std::acos(Eigen::Quaternionf(odom.block<3, 3>(0, 0)).w());

    tf::Matrix3x3 mat_rot;
    mat_rot.setValue(static_cast<double>(odom(0, 0)), static_cast<double>(odom(0, 1)),
		     static_cast<double>(odom(0, 2)), static_cast<double>(odom(1, 0)),
		     static_cast<double>(odom(1, 1)), static_cast<double>(odom(1, 2)),
		     static_cast<double>(odom(2, 0)), static_cast<double>(odom(2, 1)),
		     static_cast<double>(odom(2, 2)));
    double roll_, pitch_, yaw_;
    mat_rot.getRPY(roll_, pitch_, yaw_, 1);

    //NODELET_INFO_STREAM("roll angle is:" << roll_*57.3 << "[deg], pitch angle is:"<< pitch_*57.3 <<"[deg], yaw angle is:" <<yaw_*57.3 << "[deg].");

    if(dx > 0.1 || da > 0.06) {
      NODELET_INFO_STREAM("too large transform!!  " << dx << "[m] " << da << "[rad]");
      odom.setIdentity();
      prev_odom.setIdentity();
      //odom = prev_odom;
    }
    else      
      prev_odom = odom;

    registration_lio->setInputTarget(filtered);

    t2 = ros::WallTime::now();

    
    float para_q[4] = { 0.0, 0.0, 0.0, 1.0 };
    float para_t[3] = { 0.0, 0.0, 0.0 };

    para_q[0] = Eigen::Quaternionf( odom.block<3, 3>(0, 0) ).x();
    para_q[1] = Eigen::Quaternionf( odom.block<3, 3>(0, 0) ).y();
    para_q[2] = Eigen::Quaternionf( odom.block<3, 3>(0, 0) ).z();
    para_q[3] = Eigen::Quaternionf( odom.block<3, 3>(0, 0) ).w();

    float siny_cosp = 2 * (para_q[3] * para_q[2] + para_q[0] * para_q[1]);
    float cosy_cosp = 1 - 2 * (para_q[1] * para_q[1] + para_q[2] * para_q[2]);
    float yaw = atan2( siny_cosp, cosy_cosp );
    
    para_t[0] = odom(0, 3);
    para_t[1] = odom(1, 3);
    para_t[2] = odom(2, 3);
    
    Eigen::Map<Eigen::Quaternionf> q_last_curr(para_q);
    Eigen::Map<Eigen::Vector3f> t_last_curr(para_t);
   
    //std::cout<<"lidar odo cost time is:"<<(t2-t1).toSec()<<"sec,odo.x="<<para_t[0]<<"m, odo.y="<<para_t[1]<<"m, yaw="<<yaw * 57.3<<"deg."<<std::endl;
    
    ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp);
    
    pcl::PointCloud<PointT>::Ptr deskewed(new pcl::PointCloud<PointT>());
    deskewed->header = cloud->header;
    deskewed->is_dense = cloud->is_dense;
    deskewed->width = cloud->width;
    deskewed->height = cloud->height;
    deskewed->resize(cloud->size());

    float scan_period = private_nh.param<double>("scan_period", 0.1);
    Eigen::Quaternionf q_point_last;
    Eigen::Vector3f t_point_last;
    
    int num_per_frame = cloud->size() / 16;

    t1 = ros::WallTime::now();
    
    for(int i = 0; i < cloud->size(); i++)
      {
	
	const auto& pt = cloud->at(i);		  	

	int cur_layer = floor((i+1) / num_per_frame);

	float s = (float)((float)(i) - (float)(cur_layer * num_per_frame)) / num_per_frame;
	
	q_point_last = Eigen::Quaternionf::Identity().slerp(s, q_last_curr);
      
	t_point_last = s * t_last_curr;

	Eigen::Vector3f pt_ = q_point_last * pt.getVector3fMap() + t_point_last;

	deskewed->at(i) = cloud->at(i);
      
	deskewed->at(i).getVector3fMap() = pt_;
		      	
      }

    t2 = ros::WallTime::now();

    //std::cout<<"calcu deskew cost time is:"<<(t2-t1).toSec()<<"sec."<<std::endl;
    
    return deskewed;
  }

  /**
   * @brief downsample cloud for lidar odometry
   * @param cloud
   */
  pcl::PointCloud<PointT>::ConstPtr downsample_lio(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter_lio) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter_lio->setInputCloud(cloud);
    downsample_filter_lio->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
  }

  /**
   * @brief outlier filter for input cloud
   * @param cloud
   */
  pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!outlier_removal_filter_lio) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    outlier_removal_filter_lio->setInputCloud(cloud);
    outlier_removal_filter_lio->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
  }
  

  /**
   * @brief distance filter for input cloud
   * @param cloud
   */
  pcl::PointCloud<PointT>::ConstPtr distance_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    filtered->reserve(cloud->size());

    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) {
      double d = p.getVector3fMap().norm();
      return d > distance_near_thresh && d < distance_far_thresh;
    });

    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;

    filtered->header = cloud->header;

    return filtered;
  }

  /**
   * @brief callback for globalmap input
   * @param points_msg
   */
  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;

    registration->setInputTarget(globalmap);

    if(use_global_localization) {
      NODELET_INFO("set globalmap for global localization!");
      hdl_global_localization::SetGlobalMap srv;
      pcl::toROSMsg(*globalmap, srv.request.global_map);

      if(!set_global_map_service.call(srv)) {
        NODELET_INFO("failed to set global map");
      } else {
        NODELET_INFO("done");
      }
    }
  }

  /**
   * @brief perform global localization to relocalize the sensor position
   * @param
   */
  bool relocalize(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) {
    if(last_scan == nullptr) {
      NODELET_INFO_STREAM("no scan has been received");
      return false;
    }

    relocalizing = true;
    delta_estimater->reset();
    pcl::PointCloud<PointT>::ConstPtr scan = last_scan;

    hdl_global_localization::QueryGlobalLocalization srv;
    pcl::toROSMsg(*scan, srv.request.cloud);
    srv.request.max_num_candidates = 1;

    if(!query_global_localization_service.call(srv) || srv.response.poses.empty()) {
      relocalizing = false;
      NODELET_INFO_STREAM("global localization failed");
      return false;
    }

    const auto& result = srv.response.poses[0];

    NODELET_INFO_STREAM("--- Global localization result ---");
    NODELET_INFO_STREAM("Trans :" << result.position.x << " " << result.position.y << " " << result.position.z);
    NODELET_INFO_STREAM("Quat  :" << result.orientation.x << " " << result.orientation.y << " " << result.orientation.z << " " << result.orientation.w);
    NODELET_INFO_STREAM("Error :" << srv.response.errors[0]);
    NODELET_INFO_STREAM("Inlier:" << srv.response.inlier_fractions[0]);

    Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
    pose.linear() = Eigen::Quaternionf(result.orientation.w, result.orientation.x, result.orientation.y, result.orientation.z).toRotationMatrix();
    pose.translation() = Eigen::Vector3f(result.position.x, result.position.y, result.position.z);
    pose = pose * delta_estimater->estimated_delta();

    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    pose_estimator.reset(new hdl_localization::PoseEstimator(
      registration,
      ros::Time::now(),
      pose.translation(),
      Eigen::Quaternionf(pose.linear()),
      private_nh.param<double>("cool_time_duration", 0.5)));

    relocalizing = false;

    return true;
  }

  /**
   * @brief callback for initial pose input ("2D Pose Estimate" on rviz)
   * @param pose_msg
   */
  void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) {
    NODELET_INFO("initial pose received!!");
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    const auto& p = pose_msg->pose.pose.position;
    const auto& q = pose_msg->pose.pose.orientation;
    pose_estimator.reset(
          new hdl_localization::PoseEstimator(
            registration,
            ros::Time::now(),
            Eigen::Vector3f(p.x, p.y, p.z),
            Eigen::Quaternionf(q.w, q.x, q.y, q.z),
            private_nh.param<double>("cool_time_duration", 0.5))
    );
  }

  void heading_callback(const sensor_msgs::ImuPtr& imu_msg) {

    //fdi ccw is positive(+)
    if(imu_msg)
      {
	std::lock_guard<std::mutex> lock(q_heading_mutex);

	double yaw = tf::getYaw(imu_msg->orientation);

	yaw = atan2(sin(yaw), cos(yaw));
	yaw = (yaw - (M_PI * zero_yaw / 180.0));
	yaw = atan2(sin(yaw), cos(yaw));
	geometry_msgs::Quaternion temp_quaternion = tf::createQuaternionMsgFromYaw(yaw);
	
	q_heading.w() = temp_quaternion.w;
	q_heading.x() = temp_quaternion.x;
	q_heading.y() = temp_quaternion.y;
	q_heading.z() = temp_quaternion.z;
      }
  }
  /**
   * @brief callback for gnss input (/fix on gnss driver)
   * @param navsat_msg
   *        navsat_msg->status.status = -1 ---> gps is not ok
            navsat_msg->status.status = 0  ---> gps is in float-solve-mode
            navsat_msg->status.status = 1  ---> gps is in fix-solve-mode
   */
  void navsat_callback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) {

    //gnss_location = false;
    //if (!gnss_init || (navsat_msg->status.status == -1) || gnss_location)
    if (!gnss_init)
    {
      NODELET_INFO("gnss initialization is not OK!!!");
      return;
    }

    std::lock_guard<std::mutex> lock(q_heading_mutex);
    
    Eigen::Quaternionf q_heading_ = q_heading;

    geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
    gps_msg->header = navsat_msg->header;
    gps_msg->position.latitude = navsat_msg->latitude;
    gps_msg->position.longitude = navsat_msg->longitude;
    gps_msg->position.altitude = navsat_msg->altitude;
    
    geodesy::fromMsg(gps_msg->position, utm);
    Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

    xyz -= zero_utm;
    
    double theta = zero_yaw / 180.0 * M_PI;
    double temp_xyz0 = cos(theta)*xyz[0] + sin(theta)*xyz[1];
    double temp_xyz1 = -sin(theta)*xyz[0] + cos(theta)*xyz[1];
    xyz[0] = temp_xyz0;
    xyz[1] = temp_xyz1;
    

    geometry_msgs::Quaternion gps_temp_quaternion;
    gps_temp_quaternion.x = q_heading_.x();
    gps_temp_quaternion.y = q_heading_.y();
    gps_temp_quaternion.z = q_heading_.z();
    gps_temp_quaternion.w = q_heading_.w();
    pose_type temp_pose;//+
    temp_pose.x = xyz[0];temp_pose.y = xyz[1];temp_pose.theta = tf::getYaw(gps_temp_quaternion);//+
    temp_pose = pose_inv_transform(&temp_pose, &gps_2_lidar);//+
    temp_pose = pose_transform(&temp_pose, &gps_2_lidar);//+
    xyz[0] = temp_pose.x;//+
    xyz[1] = temp_pose.y;//+
    geometry_msgs::Quaternion temp_quaternion = tf::createQuaternionMsgFromYaw(temp_pose.theta);//+
    q_heading_.x() = temp_quaternion.x;//+
    q_heading_.y() = temp_quaternion.y;//+
    q_heading_.z() = temp_quaternion.z;//+
    q_heading_.w() = temp_quaternion.w;//+
    
    //publish RTK pose,TODO:GPS data valid or invalid jurdge
    geometry_msgs::PoseWithCovarianceStamped gps_pose;
    // Fill in the header
    gps_pose.header.frame_id = "map";
    gps_pose.header.stamp = navsat_msg->header.stamp;
    // Copy in the pose
    gps_pose.pose.pose.position.x = xyz[0];
    gps_pose.pose.pose.position.y = xyz[1];

    //geometry_msgs::Quaternion gps_temp_quaternion;
    gps_temp_quaternion.x = q_heading_.x();
    gps_temp_quaternion.y = q_heading_.y();
    gps_temp_quaternion.z = q_heading_.z();
    gps_temp_quaternion.w = q_heading_.w(); 
    
    tf2::Quaternion q;
    q.setRPY(0, 0, tf::getYaw(gps_temp_quaternion));
    tf2::convert(q, gps_pose.pose.pose.orientation);
    // Copy in the covariance, converting from 3-D to 6-D
  
    //  for(int i=0; i<2; i++)
    //  {
    //   for(int j=0; j<2; j++)
    //   {
    //     p.pose.covariance[6*i+j] = 0.1;
    //   }
    //  }
    gps_pose.pose.covariance[0] = 0.05*0.05;
    gps_pose.pose.covariance[1] = 0.0;
    gps_pose.pose.covariance[6] = 0.0;
    gps_pose.pose.covariance[7] = 0.05*0.05;     
    gps_pose.pose.covariance[6*5 + 5] = 0.01*0.01;
    gps_pose_pub.publish(gps_pose);//publish base_link's pose (in the fixed_frame) to Amcl Node

    
    if(gnss_location && globalmap)// && !(private_nh.param<bool>("specify_init_pose", false)))
      {
	std::cout<<"gps initial pose is :"<<xyz[0]<<","<<xyz[1]<<","<<tf::getYaw(gps_temp_quaternion)<<std::endl;
	
	std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
	
	pose_estimator.reset(new hdl_localization::PoseEstimator(registration, ros::Time::now(), Eigen::Vector3f(xyz[0], xyz[1], 0), q_heading_, private_nh.param<double>("cool_time_duration", 0.5)));
    
	gnss_location = false;

	NODELET_INFO("gnss_location pose received!!");

      }        
  }

  
  /**
   * @brief downsampling
   * @param cloud   input cloud
   * @return downsampled cloud
   */
  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
  }

  /**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */
  void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
    // broadcast the transform over tf
    if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) {
      geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast<double>()));
      map_wrt_frame.header.stamp = stamp;
      map_wrt_frame.header.frame_id = odom_child_frame_id;
      map_wrt_frame.child_frame_id = "map";

      geometry_msgs::TransformStamped frame_wrt_odom = tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0), ros::Duration(0.1));
      Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();

      geometry_msgs::TransformStamped map_wrt_odom;
      tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);

      tf2::Transform odom_wrt_map;
      tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
      odom_wrt_map = odom_wrt_map.inverse();

      geometry_msgs::TransformStamped odom_trans;
      odom_trans.transform = tf2::toMsg(odom_wrt_map);
      odom_trans.header.stamp = stamp;
      odom_trans.header.frame_id = "map";
      odom_trans.child_frame_id = robot_odom_frame_id;

      tf_broadcaster.sendTransform(odom_trans);
    } else {
      geometry_msgs::TransformStamped odom_trans = tf2::eigenToTransform(Eigen::Isometry3d(pose.cast<double>()));
      odom_trans.header.stamp = stamp;
      odom_trans.header.frame_id = "map";
      odom_trans.child_frame_id = odom_child_frame_id;
      tf_broadcaster.sendTransform(odom_trans);
    }

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = "map";

    tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast<double>()), odom.pose.pose);
    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

  /**
   * @brief publish scan matching status information
   */
  void publish_scan_matching_status(const std_msgs::Header& header, pcl::PointCloud<pcl::PointXYZI>::ConstPtr aligned) {
    ScanMatchingStatus status;
    status.header = header;

    status.has_converged = registration->hasConverged();
    status.matching_error = registration->getFitnessScore();

    const double max_correspondence_dist = 0.5;

    int num_inliers = 0;
    std::vector<int> k_indices;
    std::vector<float> k_sq_dists;
    for(int i = 0; i < aligned->size(); i++) {
      const auto& pt = aligned->at(i);
      registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
      if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
        num_inliers++;
      }
    }
    status.inlier_fraction = static_cast<float>(num_inliers) / aligned->size();
    status.relative_pose = tf2::eigenToTransform(Eigen::Isometry3d(registration->getFinalTransformation().cast<double>())).transform;

    status.prediction_labels.reserve(2);
    status.prediction_errors.reserve(2);

    std::vector<double> errors(6, 0.0);

    if(pose_estimator->wo_prediction_error()) {
      status.prediction_labels.push_back(std_msgs::String());
      status.prediction_labels.back().data = "without_pred";
      status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast<double>())).transform);
    }

    if(pose_estimator->imu_prediction_error()) {
      status.prediction_labels.push_back(std_msgs::String());
      status.prediction_labels.back().data = use_imu ? "imu" : "motion_model";
      status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast<double>())).transform);
    }

    if(pose_estimator->odom_prediction_error()) {
      status.prediction_labels.push_back(std_msgs::String());
      status.prediction_labels.back().data = "odom";
      status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast<double>())).transform);
    }
  
    status_pub.publish(status);
  }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;

  std::string robot_odom_frame_id;
  std::string odom_child_frame_id;

  bool use_imu;
  bool invert_acc;
  bool invert_gyro;

  bool use_gps;
  
  ros::Subscriber imu_sub;
  ros::Subscriber points_sub;
  ros::Subscriber globalmap_sub;
  ros::Subscriber initialpose_sub;
  ros::Subscriber gnss_sub;
  ros::Subscriber heading_sub;


  std::unique_ptr<message_filters::Subscriber<sensor_msgs::NavSatFix>> gps_sub_sync;
  std::unique_ptr<message_filters::Subscriber<pcl::PointCloud<PointT>>> cloud_sub_sync;
  std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;

  ros::Publisher pose_pub;
  ros::Publisher aligned_pub;
  ros::Publisher status_pub;
  ros::Publisher gps_pose_pub;
  ros::Publisher result_pose_pub;

  tf2_ros::Buffer tf_buffer;
  tf2_ros::TransformListener tf_listener;
  tf2_ros::TransformBroadcaster tf_broadcaster;

  // imu input buffer
  std::mutex imu_data_mutex;
  std::vector<sensor_msgs::ImuConstPtr> imu_data;

  // globalmap and registration method
  pcl::PointCloud<PointT>::Ptr globalmap;
  pcl::Filter<PointT>::Ptr downsample_filter;
  pcl::Registration<PointT, PointT>::Ptr registration;

  // lidar odometry
  bool first_in_lio = true;
  pcl::Filter<PointT>::Ptr downsample_filter_lio;
  pcl::Registration<PointT, PointT>::Ptr registration_lio;
  pcl::Filter<PointT>::Ptr outlier_removal_filter_lio;
  Eigen::Matrix4f prev_odom;
  Eigen::Matrix4f odom;
  bool use_distance_filter;
  double distance_near_thresh;
  double distance_far_thresh;
  
  // pose estimator
  std::mutex pose_estimator_mutex;
  std::unique_ptr<hdl_localization::PoseEstimator> pose_estimator;

  // global localization
  bool use_global_localization;
  std::atomic_bool relocalizing;
  std::unique_ptr<DeltaEstimater> delta_estimater;

  pcl::PointCloud<PointT>::ConstPtr last_scan;
  ros::ServiceServer relocalize_server;
  ros::ServiceClient set_global_map_service;
  ros::ServiceClient query_global_localization_service;

  // gnss param
  bool gnss_init = false;
  bool gnss_location = true;

  geodesy::UTMPoint utm;
  Eigen::Vector3d zero_utm;
  Eigen::Vector3d xyz;
  Eigen::Matrix3d rotMatrix;
  std::mutex q_heading_mutex;
  Eigen::Quaternionf q_heading;
  double zero_yaw;

  bool first_in_imu = true;
  bool first_in_match = true;

  double cur_gps_pose[3] = { 0.0, 0.0, 0.0 };
  double prev_gps_pose[3] = { 0.0, 0.0, 0.0 };
  double prev_pose[3] = { 0.0, 0.0, 0.0 };
  int first_in_point_cloud_aysn = true;
  
  pose_type gps_2_lidar = {0.0};

  //int relocalcount = 1;//test
};
}


PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)
